#include "../include/kuka_ft.hpp"
kuka_ft::kuka_ft(/* args */)
{
    _force =  new double[3]();
    _torque =  new double[3]();
    _force_old =  new double[3]();
    _torque_old =  new double[3]();
    _WrenchSafety = new double[6]();
    PoseTool2Sensor = new double[7]();
    _stall_counts = 0;
    ft_sensor = {0};
}

kuka_ft::~kuka_ft()
{
  delete [] _force;
  delete [] _torque;
  delete [] _force_old;
  delete [] _torque_old;
  delete [] _WrenchSafety;
  delete [] PoseTool2Sensor;
}

int kuka_ft::getWrenchSensor(double *wrench)
{
  getVrepSensor();
  
  wrench[0] = _force[0];
  wrench[1] = _force[1];
  wrench[2] = _force[2];
  wrench[3] = _torque[0];
  wrench[4] = _torque[1];
  wrench[5] = _torque[2];
  return 0;
}

int kuka_ft::getWrenchTool(double *wrench)
{
  double wrench_sensor[6];
  int flag = getWrenchSensor(wrench_sensor);
  
  // transform from sensor frame to tool frame
  Eigen::Matrix<double, 6, 1> wrench_T, wrench_S;

  wrench_S[0] = wrench_sensor[0];
  wrench_S[1] = wrench_sensor[1];
  wrench_S[2] = wrench_sensor[2];
  wrench_S[3] = wrench_sensor[3];
  wrench_S[4] = wrench_sensor[4];
  wrench_S[5] = wrench_sensor[5];

  wrench_T  = _adj_tool_2_sensor*wrench_S;
  wrench[0] = wrench_T[0];
  wrench[1] = wrench_T[1];
  wrench[2] = wrench_T[2];
  wrench[3] = wrench_T[3];
  wrench[4] = wrench_T[4];
  wrench[5] = wrench_T[5];
  return flag;
}

int kuka_ft::getWrenchNetTool(const double *pose, double *wrench_net_T)
{
  double wrench_T[6];
  int flag = getWrenchTool(wrench_T);

    // compensate for the weight of object
  Eigen::Quaterniond q(pose[3], pose[4], pose[5], pose[6]);
  Eigen::Vector3d GinF = q.normalized().toRotationMatrix().transpose()*_Gravity;
  Eigen::Vector3d GinT = -_Pcom.cross(GinF);
  wrench_net_T[0] = wrench_T[0] + _Foffset[0] - GinF[0];
  wrench_net_T[1] = wrench_T[1] + _Foffset[1] - GinF[1];
  wrench_net_T[2] = wrench_T[2] + _Foffset[2] - GinF[2];

  wrench_net_T[3] = wrench_T[3] + _Toffset[0] - GinT[0];
  wrench_net_T[4] = wrench_T[4] + _Toffset[1] - GinT[1];
  wrench_net_T[5] = wrench_T[5] + _Toffset[2] - GinT[2];

  // safety
  for (int i = 0; i < 6; ++i) {
    if(abs(wrench_net_T[i]) >_WrenchSafety[i]){
      // std::cout << "\033[1;31m[Vrep] Unsafe Data\033[0m\n";
      return 3;
    }
  }
  return flag;
}

void kuka_ft::getRealSensor()
{
  return;
}
void kuka_ft::getVrepSensor()
{
  CThread::lock();
  _force[0] = ft_sensor.x_force;
  _force[1] = ft_sensor.y_force;
  _force[2] = ft_sensor.z_force;

  _torque[0] = ft_sensor.x_torque;
  _torque[1] = ft_sensor.y_torque;
  _torque[2] = ft_sensor.z_torque;

  // printf("|<master thread force sensor>\n|%-8.5f|%-8.5f|%-8.5f|%-8.5f|%-8.5f|%-8.5f|\n",
  //   _force[0],
  //   _force[1],
  //   _force[2],
  //   _torque[0],
  //   _torque[1],
  //   _torque[2]);
  CThread::unlock();
}

void kuka_ft::init_param(){
}